#include <cstdio>

#include <memory>

#include "geometry_msgs/msg/transform_stamped.hpp"
#include <rclcpp/rclcpp.hpp>
#include <tf2_ros/static_transform_broadcaster.hpp>

// StaticBroadcaster 静态广播器
class StaticBroadcaster : public rclcpp::Node {
public:
  StaticBroadcaster() : Node("static_broadcaster") {

    // 创建静态广播器对象
    static_broadcaster_ =
        std::make_shared<tf2_ros::StaticTransformBroadcaster>(*this);

    // 创建并发布静态变换
    geometry_msgs::msg::TransformStamped t;
    // 设置时间戳、父子坐标系、平移和旋转
    t.header.stamp = this->get_clock()->now();

    // 父坐标系
    t.header.frame_id = "base_link";
    t.child_frame_id = "talk";

    // 平移
    t.transform.translation.x = 1.0;
    t.transform.translation.y = 2.0;
    t.transform.translation.z = 3.0;

    // 四元数表示旋转
    t.transform.rotation.x = 0.0;
    t.transform.rotation.y = 0.0;
    t.transform.rotation.z = 0.0;
    t.transform.rotation.w = 1.0;

    // 发送静态变换
    static_broadcaster_->sendTransform(t);
    //
    RCLCPP_INFO(this->get_logger(), "Sending static transform");
  }

private:
  std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_broadcaster_;
};

int main(int argc, char *argv[]) {
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<StaticBroadcaster>());
  rclcpp::shutdown();
  return 0;
}
